#include <stdio.h>
#include <stdlib.h>
#include <fftw3.h>
#include <complex.h>
#include <math.h>
#include <string.h>

void ifft_c2c(fftw_complex *in,fftw_complex *out,long size)
{
    fftw_plan_with_nthreads(4);
    fftw_plan plan = fftw_plan_dft_1d(size,in,out,FFTW_BACKWARD,FFTW_ESTIMATE);
    if(plan == NULL) {
        printf("ifft:Create fftw_plan error!");
        exit(1);
    }
    fftw_execute(plan);
    fftw_destroy_plan(plan);
    fftw_cleanup_threads();
}

void fft_c2r(fftw_complex *in,double *out,long size)
{
    fftw_plan_with_nthreads(4);
    fftw_plan plan = fftw_plan_dft_c2r_1d(size,in,out,FFTW_ESTIMATE);
    if(plan == NULL) {
        printf("ifft:Create fftw_plan error!");
        exit(1);
    }
    fftw_execute(plan);
    fftw_destroy_plan(plan);
    fftw_cleanup_threads();
}

void fft_r2c(double *in,fftw_complex *out,int size)
{
    fftw_plan_with_nthreads(4);
    fftw_plan plan = fftw_plan_dft_r2c_1d(size,in,out,FFTW_ESTIMATE);
    if(plan == NULL) {
        printf("fft:Create fftw_plan error!");
        exit(1);
    }
    fftw_execute(plan);
    fftw_destroy_plan(plan);
    fftw_cleanup_threads();
}

void fft_convolve(double *d1,double *d2,double *out,size_t s1,size_t s2)
{
    size_t len = s1 + s2 - 1;
    double *t1 = malloc(sizeof(double) * len);
    double *t2 = malloc(sizeof(double) * len);
    double max = 0;
    fftw_complex *freq = malloc(sizeof(fftw_complex) * len);
    fftw_complex *freq1 = malloc(sizeof(fftw_complex) * len);
    fftw_complex *freq2 = malloc(sizeof(fftw_complex) * len);


    for(size_t i = 0;i < len;++i){
        if(i < s1)
            t1[i] = d1[i];
        else
            t1[i] = 0;
    }

    for(size_t i = 0;i < len;++i){
        if(i < s2)
            t2[i] = d2[i];
        else
            t2[i] = 0;
    }

    fft_r2c(t1,freq1,len);
    fft_r2c(t2,freq2,len);
    for(size_t i = 0;i < len;++i){

        freq[i][0] = freq1[i][0] * freq2[i][0] - freq1[i][1] * freq2[i][1]; 
        freq[i][1] = freq1[i][0] * freq2[i][1] + freq1[i][1] * freq2[i][0];
    }

    fft_c2r(freq,out,len);

    for(int i = 0;i < 9;++i){
        out[i] /= len;
    }


    free(t1);
    free(t2);
    fftw_free(freq);
    fftw_free(freq1);
    fftw_free(freq2);
}

void lfilter(double *data,double *coeffs,double *out,size_t l1,size_t l2)
{
    double *res = malloc(sizeof(double) * (l1+l2-1));
    fft_convolve(data,coeffs,res,l1,l2);
    // strncpy((char *)out,(char *)res,l1 * sizeof(double) / 8);

    memcpy(out,res,l1 * sizeof(double));
    free(res);
}

int main()
{
    double a[5] = {1,2,3,4,5};
    double b[5] = {1,2,3,4,5};

    double *out = malloc(sizeof(double) * 9);
    lfilter(a,b,out,5,5);

    for(int i = 0;i < 9;++i){
        printf("%f\n",out[i]);
    }
   
    return 0;
}